#include "PathSearch.h"
#include <iostream>
#include <math.h>
using namespace std;

namespace fullsail_ai { namespace algorithms {

	// priority queue function for sorting
	template<typename T>
	bool PrioritySort(const T& lhs,const T& rhs)
	{
		return (lhs->finalCost > rhs->finalCost);
	}


	PathSearch::PathSearch():openList(&PrioritySort)
	{
		startTile = goalTile = NULL;
		tileMap = NULL;
		imDone = false;
		solution.clear();
	}

	PathSearch::~PathSearch()
	{
	}

	void PathSearch::initialize(TileMap* _tileMap)
	{
		tileMap = _tileMap;
	}

	void PathSearch::enter(int startRow, int startColumn, int goalRow, int goalColumn)
	{
		solution.clear();
		startTile = tileMap->getTile(startRow,startColumn);
		goalTile = tileMap->getTile(goalRow,goalColumn);
		PlannerNode* tempNode = new PlannerNode(startTile,0,TileDistance(startTile,goalTile));
		openList.push(tempNode);
		createdMap[startTile] = tempNode;
		imDone = false;
	}

	void PathSearch::update(long timeslice)
	{
		float time_start = (float)GetTickCount();
		while(!openList.empty())
		{
			// store the first node in the queue in current
			PlannerNode* current = openList.front();
			//cout << "This is the current Planner Node:\n";
			//cout << "Memory Address: " << current << "\n";
			//cout << "Data: " << current->m_tile << "\n";
			//cout << "Parent: " << current->parent << "\n";
			// take it out of the list
			openList.pop();
			//cout << "Popped it off the front of the queue\n";

			//cout << "Open List has: \n";
			//cout << "List size: " << openList.size() << "\n";
			//for(unsigned int listInd = 0; listInd < openList.size();++listInd)
			//{
			//	cout << "List index: " << listInd << " - ROW: " << openList[listInd]->m_tile->getRow() << " COL: "
			//		<< openList[listInd]->m_tile->getColumn() << "\n";
			//}
			//cout << "\n\n";
			
			// check to see its not the goal
			if(current->m_tile == goalTile)
			{
				// we've found the path!
				BuildSolution(current);
				imDone = true;
				cout << "Number of nodes created: " << solution.size() << "\n";
				return;
			}
			// loop through current's adjacent valid(able to move to) tiles
			// adjaceny function for the loop
			//cout << "Finding the adjacents... \n";
			FindAdjacents(current->m_tile);
			//cout << "Found this many adjacents: " << adjacents.size() << "\n\n";
			for(unsigned int i = 0; i < adjacents.size();++i)
			{
				// the temp tile set to whichever of these adjacent tiles we are on at the moment
				//cout << "Successor tile is adjacent number: " << i << "\n";
				//cout << "With memory address: " << adjacents[i] << "\n";
				Tile* successor = adjacents[i];
				float tempGivenCost = current->givenCost + (SuccessorDistance()*successor->getWeight());

				// check to see if its a duplicate
				if(!createdMap[successor])
				{
					//cout << "It is new to the created map\n";
					PlannerNode* succNode = new PlannerNode(successor,tempGivenCost,TileDistance(successor,goalTile));
					succNode->parent = current;
					//cout << "Adding succNode " << succNode << " to the map now\n";
					createdMap[successor] = succNode;
					//cout << "Putting it on the back of the openList\n\n\n";
					openList.push(succNode);
					continue;
				}
				else
				{
					// is a duplicate... check the relative costs
					if(createdMap[successor]->givenCost > tempGivenCost)
					{
						// remove the original from openList and replace it with the temp
						openList.remove(createdMap[successor]);
						createdMap[successor]->givenCost = tempGivenCost;
						createdMap[successor]->finalCost = tempGivenCost + (createdMap[successor]->heuristicCost*heurWeight);
						createdMap[successor]->parent = current;
						openList.push(createdMap[successor]);
					}
				}
				//cout << "It is already in the created map\n\n\n";
			}
#if _DEBUG
			tileMap->resetTileDrawing();

			for(map<Tile*,PlannerNode*>::iterator iter = createdMap.begin();iter != createdMap.end();++iter)
			{
				iter->first->setFill(0xFF0000FF); // blue
			}
			// to loop through the openList
			vector<PlannerNode*> tempVec;
			openList.enumerate(tempVec);
			for(unsigned int color = 0;color < tempVec.size();++color)
			{
				tempVec[color]->m_tile->setMarker(0xFFF00F00); // reddish
			}
			PlannerNode * tempNode = current;
			while(tempNode->parent)
			{
				tempNode->m_tile->setLineTo(tempNode->parent->m_tile,0xFF00F0F0); // turquoise
				tempNode = tempNode->parent;
			}
#endif
			float time_end = (float)GetTickCount();

			if(time_end - time_start >= timeslice)
				return;
		}
	}

	void PathSearch::exit()
	{
		// free all planner node memory
		for(map<Tile*,PlannerNode*>::iterator iter = createdMap.begin();iter != createdMap.end();++iter)
		{
			// delete the PlannerNode data in the map
			delete iter->second;
		}
		openList.clear();
		createdMap.clear();
		solution.clear();
	}

	void PathSearch::shutdown()
	{
		solution.clear();
		openList.clear();
		for(map<Tile*,PlannerNode*>::iterator iter = createdMap.begin();iter != createdMap.end();++iter)
		{
			// delete the PlannerNode data in the map
			delete iter->second;
		}
		createdMap.clear();
	}

	bool PathSearch::isDone() const
	{
		if(imDone)
			return true;
		else
			return false;
	}

	std::vector<Tile const*> const PathSearch::getSolution() const
	{
		std::vector<Tile const*> temp;
		if(!solution.empty())
		{
			temp = solution;
		}
		return temp;
	}

	void PathSearch::FindAdjacents(Tile* tile)
	{
		// here we will determine which tiles are the ones adjacent to the passed in tile and save them in a vector
		// to be used to loop through them and find the best possible successor.
		// temporary tile for building the vector of Tile*'s to send back
		// must clear the adjacents list every run so it only holds the latest adjeacents
		
		// clear the adjacency list
		adjacents.clear();

		Tile* tempTile;

		tempTile = tileMap->getTile(tile->getRow() - 1,tile->getColumn());
		if(tempTile != NULL && tempTile->getWeight() != 0)
			adjacents.push_back(tempTile);
		tempTile = tileMap->getTile(tile->getRow(),tile->getColumn()+1);
		if(tempTile != NULL && tempTile->getWeight() != 0)
			adjacents.push_back(tempTile);
		tempTile = tileMap->getTile(tile->getRow(),tile->getColumn()-1);
		if(tempTile != NULL && tempTile->getWeight() != 0)
			adjacents.push_back(tempTile);
		tempTile = tileMap->getTile(tile->getRow() + 1,tile->getColumn());
		if(tempTile != NULL && tempTile->getWeight() != 0)
			adjacents.push_back(tempTile);

		if(tile->getRow() % 2 == 0)
		{
			// even
			tempTile = tileMap->getTile(tile->getRow() + 1,tile->getColumn()-1);
			if(tempTile != NULL && tempTile->getWeight() != 0)
				adjacents.push_back(tempTile);
			tempTile = tileMap->getTile(tile->getRow() - 1,tile->getColumn()-1);
			if(tempTile != NULL && tempTile->getWeight() != 0)
				adjacents.push_back(tempTile);
		}
		else
		{
			// odd
			tempTile = tileMap->getTile(tile->getRow() + 1,tile->getColumn()+1);
			if(tempTile != NULL && tempTile->getWeight() != 0)
				adjacents.push_back(tempTile);
			tempTile = tileMap->getTile(tile->getRow() - 1,tile->getColumn()+1);
			if(tempTile != NULL && tempTile->getWeight() != 0)
				adjacents.push_back(tempTile);		
		}
	}

	void PathSearch::BuildSolution(PlannerNode* node)
	{
		// here we will walk all the way back to the start making a vector of the path going backwards from the goal tile
		if(node->m_tile == goalTile)
		{
			vector<Tile*> temp;

			while(node)
			{
				solution.push_back(node->m_tile);
				node = node->parent;
			}
		}
		return;
	}
	float PathSearch::TileDistance(Tile* tile1,Tile* tile2)
	{
		// we will square the distance to compare costs, so we don't have the added cost 
		// the square root operations
		double x = (tile2->getXCoordinate() - tile1->getXCoordinate());
		double y = (tile2->getYCoordinate() - tile1->getYCoordinate());

		return (float)sqrt((x*x)+(y*y));
	}
	float PathSearch::SuccessorDistance()
	{
		return (float)tileMap->getTileRadius()*2;
		
	}


}}  // namespace fullsail_ai::algorithms

